/*
 * XYController.cpp
 *
 *  Created on: Mar 27, 2011
 *      Author: Anton
 */

#include <motion/XYController.h>
#include <math.h>

XYController::XYController(Gait* XY) {
	gait = XY;
}

XYController::~XYController() {
}

void XYController::setCurrentX(double X) {
	currentX = X;
}
void XYController::setCurrentY(double Y) {
	currentY = Y;
}
void XYController::setDesiredX(double X) {
	desiredX = X;
}
void XYController::setDesiredY(double Y) {
	desiredY = Y;
}

int XYController::getI() const {
	return i;
}

void XYController::setI(int i) {
	this->i = i;
}

double XYController::getDa() {
	return da;
}

double XYController::getA() {
	return a;
}

double XYController::getDx() {
	return dx;
}

double XYController::getDy() {
	return dy;
}

int XYController::calculateIterations() {

	dx = sin((desiredX - currentX) / sqrt(pow((desiredX - currentX), 2) + pow(
			(desiredY - currentY), 2)) * gait->getWidth());
	dy = cos((desiredY - currentY) / sqrt(pow((desiredX - currentX), 2) + pow(
			(desiredY - currentY), 2)) * gait->getLength());
	a = sqrt(pow((desiredX - currentX), 2) + pow((desiredY - currentY), 2));
	da = sqrt(pow(dx, 2) + pow(dy, 2));

	i = (int) (ceil(a / da));
	return i;
}

void XYController::calculateVectors(){

	gait->rbe->setFl( sin((desiredX - currentX) / sqrt(pow((desiredX - currentX), 2) + pow(
			(desiredY - currentY), 2))) );
	gait->rbe->setBl( sin((desiredX - currentX) / sqrt(pow((desiredX - currentX), 2) + pow(
			(desiredY - currentY), 2))) );
	gait->rbe->setVl( cos((desiredY - currentY) / sqrt(pow((desiredX - currentX), 2) + pow(
			(desiredY - currentY), 2))) );
}
